classdef ImprovedKalmanFilter
    properties
        state     % 状态向量 [x, y, vx, vy]
        F         % 状态转移矩阵
        B         % 控制输入矩阵
        H         % 观测矩阵
        P         % 状态协方差矩阵
        Q         % 过程噪声协方差
        R         % 观测噪声协方差
    end
    
    methods
        function obj = ImprovedKalmanFilter(dt, initialState)
            % 使用提供的初始状态初始化
            obj.state = initialState;
            
            % 状态转移矩阵 (匀速模型)
            obj.F = [1 0 dt 0;
                     0 1 0 dt;
                     0 0 1  0;
                     0 0 0  1];
            
            % 控制输入矩阵 (加速度到速度)
            obj.B = [0.5*dt^2 0;
                     0 0.5*dt^2;
                     dt 0;
                     0 dt];
            
            % 观测矩阵 (只观测位置)
            obj.H = [1 0 0 0;
                     0 1 0 0];
            gnss_noise = 1.0;
            % 初始协方差矩阵 - 根据初始状态的不确定性设置
            % 位置不确定性: 1m, 速度不确定性: 0.5m/s
            position_uncertainty = 1;   % 米
            velocity_uncertainty = 0.5; % 米/秒
            obj.P = diag([position_uncertainty^2, position_uncertainty^2, ...
                          velocity_uncertainty^2, velocity_uncertainty^2]);
            
            % 过程噪声协方差 - 与系统动态相关
            % 加速度噪声影响: 0.1 m/s^2
            accel_noise_effect = 0.1;
            obj.Q = diag([(0.5*dt^2*accel_noise_effect)^2, ... % x位置噪声
                          (0.5*dt^2*accel_noise_effect)^2, ... % y位置噪声
                          (dt*accel_noise_effect)^2, ...        % x速度噪声
                          (dt*accel_noise_effect)^2]);          % y速度噪声
            
            % 观测噪声协方差
            obj.R = diag([gnss_noise^2, gnss_noise^2]);
        end
        
        function obj = predict(obj, accel)
            % 状态预测
            obj.state = obj.F * obj.state + obj.B * accel(:);
            
            % 协方差预测
            obj.P = obj.F * obj.P * obj.F' + obj.Q;
        end
        
        function obj = update(obj, measurement)
            % 计算卡尔曼增益
            S = obj.H * obj.P * obj.H' + obj.R;
            K = obj.P * obj.H' / S;
            
            % 状态更新
            y = measurement(:) - obj.H * obj.state;
            obj.state = obj.state + K * y;
            
            % 协方差更新 (Joseph形式，数值更稳定)
            I = eye(size(obj.P));
            obj.P = (I - K * obj.H) * obj.P * (I - K * obj.H)' + K * obj.R * K';
        end
    end
end
